﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Robotics.Race
{
	public class Car
	{
		#region Properties

		private Controller driver;
		public Controller Driver { get { return driver; } set { driver = value; } }

		private NxtConfiguration nxt;
		public NxtConfiguration Nxt { get { return nxt; } }

		private double speed = 0;
		public double Speed { get { return speed; } set { speed = value; } }

		private int maxSpeed = 100;
		public int MaxSpeed { get { return maxSpeed; } }

        public int StandingTurnSpeed { get { return GlobalSettings.Instance.StandingTurnSpeed; } }

        public double TurnFactor { get { return GlobalSettings.Instance.TurnFactor; } }

        public double AccelerationRatio { get { return GlobalSettings.Instance.AccelerationRatio; } }

        public double FrictionRatio { get { return GlobalSettings.Instance.FrictionRatio; } }

		ControlGear controlGear = new ControlGear();
		public ControlGear ControlGear { get { return controlGear; } }

        public int UpdateIntervalMs { get { return 75; } }
        public int SenseIntervalMs { get { return 75; } }

		private string name;
		public string Name { get { return name; } set { name = value; } }

		private SensedEnvironment environment = new SensedEnvironment();
		public SensedEnvironment Environment { get { return environment; } }

		#endregion

		#region Constructors

		public Car( NxtConfiguration nxt )
		{
			this.nxt = nxt;
            Name = "Béla";
			Initialize();
		}

		public Car( NxtConfiguration nxt, Controller controller )
		{
			this.nxt = nxt;
			this.Driver = controller;
			Initialize();
		}

		#endregion

        public void CloseThread()
        {
            if (updaterThread != null)
                updaterThread.Abort();
        }

		~Car()
		{
            CloseThread();
		}

		public void Initialize()
		{
			Nxt.PressureSensor.AutoPoll = true;
			Nxt.PressureSensor.AutoPollDelay = SenseIntervalMs;
			Nxt.PressureSensor.ValueChanged += new Bram.NxtSharp.SensorEvent( PressureSensor_ValueChanged );

			Nxt.LightSensorLeft.AutoPoll = true;
			Nxt.LightSensorLeft.AutoPollDelay = SenseIntervalMs;
			Nxt.LightSensorLeft.ValueChanged += new Bram.NxtSharp.SensorEvent( LightSensorLeft_ValueChanged );

			Nxt.LightSensorRight.AutoPoll = true;
			Nxt.LightSensorRight.AutoPollDelay = SenseIntervalMs;
			Nxt.LightSensorRight.ValueChanged += new Bram.NxtSharp.SensorEvent( LightSensorRight_ValueChanged );
			
			Nxt.Sonar.AutoPoll = true;
			Nxt.Sonar.AutoPollDelay = SenseIntervalMs;
			Nxt.Sonar.ValueChanged += new Bram.NxtSharp.SensorEvent( Sonar_ValueChanged );

			if ( updaterThread != null )
				updaterThread.Abort();
			updaterThread = new System.Threading.Thread( UpdateLoop );
			updaterThread.Start();
		}

		System.Threading.Thread updaterThread = null;

		void Sonar_ValueChanged( Bram.NxtSharp.NxtSensor sensor )
		{
			environment.Distance = Nxt.Sonar.RawValue;
		}

		void PressureSensor_ValueChanged( Bram.NxtSharp.NxtSensor sensor )
		{
			environment.Touch = Nxt.PressureSensor.LastResult.ScaledValue == 1;
		}

		void LightSensorLeft_ValueChanged( Bram.NxtSharp.NxtSensor sensor )
		{
			environment.LeftLight = Nxt.LightSensorLeft.Value;
            if (environment.LeftLight < rules.TerrainRule.TerrainBoundary)
                environment.LeftTerrain = SensedEnvironment.TerrainType.Grass;
            else
                environment.LeftTerrain = SensedEnvironment.TerrainType.Road;
        }

		void LightSensorRight_ValueChanged( Bram.NxtSharp.NxtSensor sensor )
		{
			environment.RightLight = Nxt.LightSensorRight.Value;
            if (environment.RightLight < rules.TerrainRule.TerrainBoundary)
                environment.RightTerrain = SensedEnvironment.TerrainType.Grass;
            else
                environment.RightTerrain = SensedEnvironment.TerrainType.Road;
        }

		public void Update()
		{
            if (Nxt.Brick != null && Nxt.Brick.IsConnected)
            {
                Nxt.PressureSensor.Poll();
                Nxt.LightSensorLeft.Poll();
                Nxt.LightSensorRight.Poll();
                Nxt.Sonar.Poll();
            }
            if (Driver != null)
				Driver.Drive( this.Environment, ControlGear );


    		//double constraintedSpeed = GetConstraintedSpeed( distance );

            if (rules.TouchRule.Enabled)
                rules.TouchRule.Apply(this);

            if (rules.SonarRule.Enabled)
                rules.SonarRule.Apply(this);

            if (rules.TerrainRule.Enabled)
                rules.TerrainRule.Apply(this);

            double speedA = Speed;
			double speedB = Speed;

            speed = Math.Sign(Speed) * Math.Max(Math.Abs(Speed) - FrictionRatio, 0);
			speed = Math.Min( Math.Max( Speed + ControlGear.Acceleration * AccelerationRatio, -maxSpeed ), maxSpeed );
			speedA = speedB = Speed;

            environment.Speed = speed;

			if ( speed == 0 )
			{
				speedA += StandingTurnSpeed * ControlGear.Steering;
				speedB -= StandingTurnSpeed * ControlGear.Steering;
			}
			else
			{
                if (ControlGear.Steering < 0)
                {
                    speedA *= 1 - (1 - TurnFactor) * -ControlGear.Steering;
                    speedB *= 1 + (1 - Math.Sqrt(TurnFactor)) * -ControlGear.Steering;
                }
                else if (ControlGear.Steering > 0)
                {
                    speedA *= 1 + (1 - Math.Sqrt(TurnFactor)) * ControlGear.Steering;
                    speedB *= 1 - (1 - TurnFactor) * ControlGear.Steering;
                }
                speedA = Math.Min(MaxSpeed, speedA);
                speedB = Math.Min(MaxSpeed, speedB);
            }

			if ( ControlGear.Handbrake )
			{
				speedA = speedB = speed = 0;
			}

            if (paused)
                Stop();
            else if (Nxt.Brick != null && Nxt.Brick.IsConnected)
			{
				Nxt.MotorLeft.Turn( (int)speedA, 0 );
				Nxt.MotorRight.Turn( (int)speedB, 0 );
			}

			UpdatedEvent( this );
		}

        public void Stop()
        {
            if (Nxt.Brick.IsConnected)
            {
                Nxt.MotorRight.Brake();
                Nxt.MotorLeft.Brake();
                Speed = 0;
            }
        }

        public void TurnOffLights()
        {
            if (Nxt.Brick.IsConnected)
            {
                if (Nxt.LightSensorLeft != null)
                {
                    Nxt.LightSensorLeft.Active = false;
                }
                if (Nxt.LightSensorRight != null)
                {
                    Nxt.LightSensorRight.Active = false;
                }
            }
        }

        private Rules rules = null;
        public void SetRules(Rules rules)
        {
            this.rules = rules;
        }

        object pausedLock = new object();
        public void Pause()
        {
            paused = true;
            Stop();
        }

        public void Resume()
        {
            paused = false;
        }

		public event UpdatedHandler UpdatedEvent;
		public delegate void UpdatedHandler( Car car );

        public bool paused = false;

		public void UpdateLoop()
		{
			while ( true )
			{
				System.Threading.Thread.Sleep(UpdateIntervalMs);
                if (!paused)
                    Update();
			}
		}
	}
}
